gxbb: Workaround for PSCI_CPU_OFF

There seems to be a problem where SCP can't turn CPU0 off. Instead of
returning PSCI_E_DENIED or crashing make CPU0 wait in a WFE loop. This
way all CPUs have a consistent behaviour from the point of view of the
caller.

Change-Id: I5c8c266ca3b69c9e7a4f5ae70afeea5dd36a0825
Signed-off-by: Antonio Nino Diaz <[email protected]>

diff --git a/plat/meson/gxbb/gxbb_pm.c b/plat/meson/gxbb/gxbb_pm.c
index 632a53f46c43241d484b0a37be6347c66c036fdb..930b5e168852b9028921bf3fdaf00f68be400104 100644 (file)
--- a/plat/meson/gxbb/gxbb_pm.c
+++ b/plat/meson/gxbb/gxbb_pm.c
@@ -25,6 +25,7 @@
 #define SCPI_SYSTEM_REBOOT     1
 
 static uintptr_t gxbb_sec_entrypoint;
+static volatile uint32_t gxbb_cpu0_go;
 
 static void gxbb_program_mailbox(u_register_t mpidr, uint64_t value)
 {
@@ -83,6 +84,22 @@ static void __dead2 gxbb_system_off(void)
 
 static int32_t gxbb_pwr_domain_on(u_register_t mpidr)
 {
+       unsigned int core = plat_gxbb_calc_core_pos(mpidr);
+
+       /* CPU0 can't be turned OFF, emulate it with a WFE loop */
+       if (core == GXBB_PRIMARY_CPU) {
+               VERBOSE("BL31: Releasing CPU0 from wait loop...\n");
+
+               gxbb_cpu0_go = 1;
+               flush_dcache_range((uintptr_t)&gxbb_cpu0_go, sizeof(gxbb_cpu0_go));
+               dsb();
+               isb();
+
+               sev();
+
+               return PSCI_E_SUCCESS;
+       }
+
        gxbb_program_mailbox(mpidr, gxbb_sec_entrypoint);
        scpi_set_css_power_state(mpidr,
                                 SCPI_POWER_ON, SCPI_POWER_ON, SCPI_POWER_ON);
@@ -94,9 +111,18 @@ static int32_t gxbb_pwr_domain_on(u_register_t mpidr)
 
 static void gxbb_pwr_domain_on_finish(const psci_power_state_t *target_state)
 {
+       unsigned int core = plat_gxbb_calc_core_pos(read_mpidr_el1());
+
        assert(target_state->pwr_domain_state[MPIDR_AFFLVL0] ==
                                        PLAT_LOCAL_STATE_OFF);
 
+       if (core == GXBB_PRIMARY_CPU) {
+               gxbb_cpu0_go = 0;
+               flush_dcache_range((uintptr_t)&gxbb_cpu0_go, sizeof(gxbb_cpu0_go));
+               dsb();
+               isb();
+       }
+
        gicv2_pcpu_distif_init();
        gicv2_cpuif_enable();
 }
@@ -112,10 +138,37 @@ static void gxbb_pwr_domain_off(const psci_power_state_t *target_state)
 
        gicv2_cpuif_disable();
 
+       /* CPU0 can't be turned OFF, emulate it with a WFE loop */
+       if (core == GXBB_PRIMARY_CPU)
+               return;
+
        scpi_set_css_power_state(mpidr,
                                 SCPI_POWER_OFF, SCPI_POWER_ON, SCPI_POWER_ON);
 }
 
+static void __dead2 gxbb_pwr_domain_pwr_down_wfi(const psci_power_state_t
+                                                *target_state)
+{
+       unsigned int core = plat_gxbb_calc_core_pos(read_mpidr_el1());
+
+       /* CPU0 can't be turned OFF, emulate it with a WFE loop */
+       if (core == GXBB_PRIMARY_CPU) {
+               VERBOSE("BL31: CPU0 entering wait loop...\n");
+
+               while (gxbb_cpu0_go == 0)
+                       wfe();
+
+               VERBOSE("BL31: CPU0 resumed.\n");
+
+               write_rmr_el3(RMR_EL3_RR_BIT | RMR_EL3_AA64_BIT);
+       }
+
+       dsbsy();
+
+       for (;;)
+               wfi();
+}
+
 /*******************************************************************************
  * Platform handlers and setup function.
  ******************************************************************************/
@@ -123,6 +176,7 @@ static const plat_psci_ops_t gxbb_ops = {
        .pwr_domain_on                  = gxbb_pwr_domain_on,
        .pwr_domain_on_finish           = gxbb_pwr_domain_on_finish,
        .pwr_domain_off                 = gxbb_pwr_domain_off,
+       .pwr_domain_pwr_down_wfi        = gxbb_pwr_domain_pwr_down_wfi,
        .system_off                     = gxbb_system_off,
        .system_reset                   = gxbb_system_reset,
 };
@@ -132,5 +186,6 @@ int plat_setup_psci_ops(uintptr_t sec_entrypoint,
 {
        gxbb_sec_entrypoint = sec_entrypoint;
        *psci_ops = &gxbb_ops;
+       gxbb_cpu0_go = 0;
        return 0;
 }